#include "stm32f4xx.h"                  // Device header
#include "MOTOR/Encoder.h"
#include "TIMER/Timer.h"
#define pai		3.142



void Encoder_Init(Encoder* encoder)
{
	GPIO_InitTypeDef GPIO_InitStructure;
	TIM_TimeBaseInitTypeDef TIM_TimerStructure;
	TIM_ICInitTypeDef  TIM_InitStructure;
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
	RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA,ENABLE);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB,ENABLE);
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOC,ENABLE);
	
	GPIO_InitStructure.GPIO_Mode =GPIO_Mode_AF;        //复用
	GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_DOWN;	// 下拉
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_5;
	GPIO_Init(GPIOA,&GPIO_InitStructure);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
	GPIO_Init(GPIOB,&GPIO_InitStructure);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 ;
	GPIO_Init(GPIOC,&GPIO_InitStructure);

	GPIO_PinAFConfig(GPIOC,GPIO_PinSource6,GPIO_AF_TIM3);
	GPIO_PinAFConfig(GPIOC,GPIO_PinSource7,GPIO_AF_TIM3);
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource5,GPIO_AF_TIM2);
	GPIO_PinAFConfig(GPIOB,GPIO_PinSource3,GPIO_AF_TIM2);

	TIM_TimerStructure.TIM_ClockDivision = TIM_CKD_DIV1;        //指定时钟分频，TIM_CKD_DIV1为1分频
	TIM_TimerStructure.TIM_CounterMode = TIM_CounterMode_Up;    //配置计数器模式
	TIM_TimerStructure.TIM_Period = 65536 - 1;					//ARR自动重装器的事件周期
	TIM_TimerStructure.TIM_Prescaler = 1 - 1;                //PSC预分频器的值     CNT计数器和预分频器PSC有一个值的偏差
	TIM_TimerStructure.TIM_RepetitionCounter = 0;               //配置高级计时器时用
	TIM_TimeBaseInit(TIM3,&TIM_TimerStructure);                 //初始化时基单元
	TIM_TimeBaseInit(TIM2,&TIM_TimerStructure);                 //初始化时基单元

	TIM_EncoderInterfaceConfig(TIM2,TIM_EncoderMode_TI1,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);

	TIM_ICStructInit(&TIM_InitStructure);	
	TIM_InitStructure.TIM_Channel = TIM_Channel_1;           		//选择输入捕获通道三
	TIM_InitStructure.TIM_ICFilter = 0xF;                    		//滤波
	TIM_ICInit(TIM2,&TIM_InitStructure);
	
	TIM_ICStructInit(&TIM_InitStructure);
	TIM_InitStructure.TIM_Channel = TIM_Channel_2;           		//选择输入捕获通道四
	TIM_InitStructure.TIM_ICFilter = 0xF;                    		//滤波
	TIM_ICInit(TIM2,&TIM_InitStructure);	
	
	TIM_EncoderInterfaceConfig(TIM3,TIM_EncoderMode_TI1,TIM_ICPolarity_Rising,TIM_ICPolarity_Rising);

	TIM_ICStructInit(&TIM_InitStructure);
	TIM_InitStructure.TIM_Channel = TIM_Channel_1;           		//选择输入捕获通道一
	TIM_InitStructure.TIM_ICFilter = 0xF;                    		//滤波
	TIM_ICInit(TIM3,&TIM_InitStructure);	
	
	TIM_ICStructInit(&TIM_InitStructure);
	TIM_InitStructure.TIM_Channel = TIM_Channel_2;           		//选择输入捕获通道二
	TIM_InitStructure.TIM_ICFilter = 0xF;                    		//滤波
	TIM_ICInit(TIM3,&TIM_InitStructure);
	
	
	
	TIM_Cmd(TIM2,ENABLE);
	TIM_Cmd(TIM3,ENABLE);
	
	encoder->param.ppr = 500;
	encoder->param.d = 65;
	encoder->param.reduction_ratio = 28;
	TIM7_Init(300);	//30ms定时中断
}

void Get_Encoder(Encoder *encoder)
{
	int16_t count = 0;
	count = TIM_GetCounter(TIM3);
	encoder->Counter_L1 = count;
	count = TIM_GetCounter(TIM2);
	encoder->Counter_R1 = count;
	TIM_SetCounter(TIM2,0);
	TIM_SetCounter(TIM3,0);
}

void Get_Speed(Encoder *encoder)
{
	Get_Encoder(encoder);
	// encoder->param.ppr * encoder->param.reduction_ratio = 500 * 28 = 14000	电机一圈的脉冲
	// 实际速度 = ((实测脉冲 / 28000) * 3.142 * 0.065) / 0.03 = 实测脉冲 * 0.00048626
	encoder->Speed_L1 = encoder->Counter_L1 * 0.00024313;
	encoder->Speed_R1 = encoder->Counter_R1 * 0.00024313;
}
